#include <stdio.h>
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "auto.h"

#define LEFT_MOTOR pwm01
#define RIGHT_MOTOR pwm02


#define DONE 0
#define WAIT 1
#define FORWARD 2
#define LEFT 3
#define RIGHT 4
#define BACK 5
#define ARMOUT 6
#define ARMIN 7
#define FSTOUT 8
#define FSTIN 9
#define FORWARDFULL 10
#define HARDLEFT 11



int curr = 0;

int instList[] = {FORWARDFULL, HARDLEFT, FORWARDFULL, HARDLEFT, FORWARDFULL, HARDLEFT, FORWARDFULL, HARDLEFT, FORWARDFULL, DONE};
int timeList[] = {41         ,        9,          11,        9,          36,        9,          11,        9,          40,    1}; //38.4 per second - WHOLE NUMBERS ONLY
//*********TESTING ONLY**************
//int instList[] = {FORWARDFULL, HARDLEFT, FORWARD, DONE};
//int timeList[] = {7, 9, 2, 1}; //38.4 per second - WHOLE NUMBERS ONLY

void auto_loop() {
  static int ticksLeft = -1;
  static int pos = -1;
  static char done = FALSE;

  LEFT_MOTOR = RIGHT_MOTOR = 127;

  if (done)
    return;

  if (ticksLeft <= 0) {
    pos++;
    ticksLeft = timeList[pos]*4;
  }

  if (instList[pos] == DONE) {
    done = TRUE;
    return;
  }

  switch (instList[pos]) {
    case FORWARD:
      forward();
      break;
    case LEFT:
      left();
      break;
    case RIGHT:
      right();
      break;
    case BACK:
      back();
      break;
	case ARMOUT:
	  armout();
	  break;
	case ARMIN:
	  armin();
	  break;
	case FSTOUT:
	  fstout();
	  break;
	case FSTIN:
	  fstin();
	  break;
	case FORWARDFULL:
	  forwardfull();
	  break;
	case HARDLEFT:
	  hardleft();
	  break;
    case WAIT:
    case DONE:
      break;
    }

  ticksLeft--;
}

void forward() {
  LEFT_MOTOR = 127 + 80;
  RIGHT_MOTOR = 127 - 80;
}

void left() {
  LEFT_MOTOR = 127 - 40;
  RIGHT_MOTOR = 127 - 40;
}

void right() {
  LEFT_MOTOR = 127 + 40;
  RIGHT_MOTOR = 127 + 40;
}

void back() {
  LEFT_MOTOR = 127 - 80;
  RIGHT_MOTOR = 127 + 80;
}

void armout() {
  relay5_fwd = 1;
  relay6_fwd = 0;
}

void armin() {
  relay5_fwd = 0;
  relay6_fwd = 1;
}

void fstout() {
  relay7_fwd = 1;
  relay8_fwd = 0;
}

void fstin() {
  relay7_fwd = 0;
  relay8_fwd = 1;
}

void forwardfull() {
  LEFT_MOTOR = 255;
  RIGHT_MOTOR = 1;
}

void hardleft() {
  LEFT_MOTOR = 1;
  RIGHT_MOTOR = 1;
}
	

/*
void auto_loop() {

//Digital ins used are 2, 3, 4, and 5, representing forward, reverse, left, and right, respectively.
//That means that they should be set up like that when we program the IR board

  char fwdpress = FALSE, revpress = FALSE, leftpress = FALSE, rightpress = FALSE;
//Check to see if forward is pressed
  fwdpress = rc_dig_in02;

//Check to see if reverse is pressed, and make sure that forward is not pressed at the same time
  revpress = rc_dig_in03 && !fwdpress;

//Check to see if Left is pressed
  leftpress = rc_dig_in04;

//Check to see if Right is pressed, and make sure that Left is not pressed at the same time
  rightpress = rc_dig_in05 && !leftpress;

// Here is the fun part where we pass motor commands
  MOTOR_RIGHT = MOTOR_LEFT = 127;
//FORWARD
  if (fwdpress) {
    if (leftpress) {
      MOTOR_LEFT = 195;
      MOTOR_RIGHT = (255-147);
    } else if (rightpress) {
      MOTOR_LEFT = 147;
      MOTOR_RIGHT = (255-195);
    } else {
      MOTOR_LEFT = 195;
      MOTOR_RIGHT = (255-195);
    }
  } else if (revpress) {
    if (!leftpress&&!rightpress) {
      MOTOR_LEFT = 70;
      MOTOR_RIGHT = (255-70);
    }
    if (leftpress) {
      MOTOR_LEFT = 70;
      MOTOR_RIGHT = (255-100);
    } else if (rightpress) {
      MOTOR_LEFT = 100;
      MOTOR_RIGHT = (255-70);
    } else {
      MOTOR_LEFT = 70;
      MOTOR_RIGHT = (255-70);
    } 
  } else {
    if (leftpress) {
      MOTOR_LEFT = 127;
      MOTOR_RIGHT = (255-195);
    } else if (rightpress) {
      MOTOR_LEFT = 195;
      MOTOR_RIGHT = 127;
    }
  }
}

*/
